from AStar.AStar import AStar, SQ_Location, SQ_MapHandler

import math


def ASTAR_UTIL_QMap(siz,val):
    """ Returns a table filled with (val) of dimension siz
    this table is compatible with the internal format of AStarMap"""
    w,h=siz
    tabl=[val]*(w*h)
    tabl.append(siz)
    return tabl

class AStarMap(object):
    MAP_BLANK=0
    MAP_FORBIDDEN=255

    def __init__(self,dim=(1,1),iWorldDim=(100,100)):

        (w,h)=self.dimensions=dim
         
        self.data=[self.MAP_BLANK]*(w*h)
        #print "Dimensions : "+str(dim)
        #print "World dim : "+str(iWorldDim)
        
        self.worldDim=iWorldDim
        self.scaleFactor=tuple([((1.0)*x)/dx for (x,dx) in zip(dim,iWorldDim)])
        print " Scale : "+str(self.scaleFactor)



    def clear(self,color=None):
        if not color:
            color=self.MAP_BLANK
        
        color=max(0,min(0xFF,color))
        
        
        (w,h)=self.dimensions
        self.data[:]=[self.MAP_BLANK]*(w*h)
        self.data.append(self.dimensions)
    
    def getDimensions(self):
        return self.dimensions

    def debugPath(self,path):
        lp=[self.getMapCoord(p) for p in path]
        
        (w,h)=self.dimensions
        
        lmap=[0]*(w*h)
        for (x,y) in lp:
            lmap[y*w+x]=1
            
        for i in range(h):
            s="."
            for j in range(i*w,i*w+w):
                if self.data[j]!=0:
                    if lmap[j]!=0:
                        s+="*"
                    else :
                        s+="X"
                    
                    
                else:
                    if lmap[j]!=0:
                        s+="o"
                    else :
                        s+=" "
                    
            s+="."
            print s
         
    
    def getRangeIndex(self,zone=None,w=0):
        coord=[]
        if not zone:
            zone=((0,0),self.dimensions)
            w=self.dimensions[0]
        mini=[min(max(a,0),b) for a,b in zip(zone[0],self.dimensions)]
        maxi=[min(max(a,0),b) for a,b in zip(zone[1],self.dimensions)]
        for i in range(mini[1],maxi[1]):
            for j in range(i*w+mini[0],i*w+maxi[0]):
                coord.append(j)
        return tuple(coord)
    
    def multiply(self,factor,zone=None,w=0):
        
        
        for i in self.getRangeIndex(zone):
            v=int(self.data[i]*factor)
            if v>0xFF: v=0xFF;
            if v<0:v=0;
            
            self.data[i]=v
                
    def getIndexes(self,iMap,iTopLeft):
        x,y=self.getMapCoord(iTopLeft)
        w,h=iMap[-1]
        index1=self.getRangeIndex(((x,y),(x+w,y+h)),self.dimensions[0])
        index2=self.getRangeIndex(((0,0),(w,h)),w)    
        return index1,index2
                
    def addZone(self,iMap,iTopLeft):
        """ adds a zone presented as a 1dim table v[x+y*w]
        with dimensions as last item (w,h) to the map
        at topleft point iTopLeft in World coord"""
        index1,index2=self.getIndexes(iMap,iTopLeft)
        for i in range(len(index1)):
            v=max(0,min(0xFF,self.data[index1[i]]+iMap[index2[i]]))
            self.data[index1[i]]=v
    
    def mulZone(self,iMap,iTopLeft):
        """ adds a zone presented as a 1dim table v[x+y*w]
        with dimensions as last item (w,h) to the map
        at topleft point iTopLeft in World coord"""
        index1,index2=self.getIndexes(iMap,iTopLeft)
        for i in range(len(index1)):
            v=max(0,min(0xFF,self.data[index1[i]]*iMap[index2[i]]))
            self.data[index1[i]]=int(v)
            
    def debugPrint(self):
        (w,h)=self.dimensions
        
        for i in range(h):
            s="."
            for j in range(i*w,i*w+w):
                if self.data[j]!=0:
                    s+=str(self.data[j])
                else:
                    s+=" "
                    
            s+="."
            print s
            
    def drawLineWorld(self,pts,iBlackValue):
        local_pts=[self.getMapCoord(p) for p in pts]
        self.drawLine(local_pts,iBlackValue)

    def drawWorldPolygons(self, pol,iBlackValue):
        for p in pol:
            self.drawWorldPolygon(p,iBlackValue)
    
    def drawWorldPolygon(self, pts,iBlackValue):
        lines=[]
        n=len(pts)-1
        local_pts=[self.getMapCoord(p) for p in pts]
        for i in range(n):
            lines.append((local_pts[i],local_pts[i+1]))
        
        lines.append((local_pts[n],local_pts[0]))
    
        for l in lines:
            self.drawLine(l,iBlackValue)
        
    def drawLine(self,pts,iBlackValue):
        """Draws a line in a binary 2 dimentional array.
        The line is drawn so that no diagonal is used (leave no blank space if a barrier is drawn)."""
        ((xi,yi),(xf,yf)) = pts
        
        
        dx = xf - xi
        dy = yf - yi

        x=xi
        y=yi
        

        if dx > 0 : xinc=1;
        else : xinc=-1;
        
        if dy > 0 : yinc=1;
        else : yinc=-1;
       
        dx=math.fabs(dx)
        dy=math.fabs(dy)

        self.setValue((x,y),iBlackValue)
        if dx > dy:
            cumul = dx / 2
            for i in range(1,int(dx)+1):
                x = x+xinc
                cumul = cumul+dy

                if cumul >= dx:
                
                    cumul = cumul- dx
                    self.setValue((x,y),iBlackValue) # to avoid empty diagonals
                    y = y+ yinc
                
                self.setValue((x,y),iBlackValue)
        
        else:
            cumul = dy / 2
            for i in range(1,int(dy)+1):
            
                y =y+ yinc
                cumul = cumul + dx

                if cumul >= dy :
                
                    cumul = cumul - dy
                    self.setValue((x,y),iBlackValue)
                    x = x + xinc
                
                self.setValue((x,y),iBlackValue)


    def getValue(self, iP):
        """ Takes a tuple (x,y) as argument"""
        
        (x,y)=iP
        (w,h)=self.getDimensions()
        x = min(x, w-1)
        y = min(y, h-1)
       
        if x < 0 : x= 0
        if y < 0 : y= 0
         
        return self.data[x+y*w]

    def setValue(self,iP,iVal):
        (x,y)=iP
        (w,h)=self.getDimensions()
        x = min(x, w-1)
        y = min(y, h-1)
        
        if x < 0 : x= 0
        if y < 0 : y= 0
        
        self.data[x+y*w]=iVal
        
        

    def getMapCoord(self, ipos):
        """ Returns a position on the map corresponding to a (x,y) in the world
        Takes a tuple as argument"""
        w,h=self.getDimensions()
        if not w*h==0:
        
            return  tuple([int(a*b) for a,b in zip(ipos,self.scaleFactor)])
        
        return None

    def getWorldCoord(self, iP):
        """ Takes a tuple(x,y) like as argument"""
        w,h=self.getDimensions()
            
        if not w*h==0:
            return  tuple([int((1.0*a)/b) for a,b in zip(iP,self.scaleFactor)])
        
        return None

    def compute2DPath(self,iStart, iGoal):
        """ Calculates the A* path between the two (x,y) map positions as argument,
        using iMap as weight map"""
        (w,h)=self.getDimensions()
        start=self.getMapCoord(iStart)
        end=self.getMapCoord(iGoal)
        print "Map coord :"
        print start
        print end
        mh=SQ_MapHandler(self.data,w,h,self.MAP_FORBIDDEN)
        astar = AStar(mh)
        
        p = astar.findPath(start,end)
        
        lwp=None
        
        oDx=0
        oDy=0
        dx=0
        dy=0
        oX=-20000
        oY=-20000
        lastPoint=None
                
        
        if p:
            lwp=[]
            #print p
            for pos in p:
                currentPoint=self.getWorldCoord(pos)
                oDx = dx
                oDy = dy
                (x,y)=pos
                dx = x - oX
                dy = y - oY
                oX = x
                oY = y
                if lastPoint:
                    if ((dx == oDx)and(dy == oDy)):
                        del lwp[-1]
                lastPoint = currentPoint
                    
                lwp.append(currentPoint)
        return lwp
 

if __name__=="__main__":
    from decodeSVG.decodeSVG import SVGParser
    import os.path
    
    path=os.path.join("res","simple.svg")
    pData=open(path)
    xmlstr=pData.read()
    svg=SVGParser(xmlstr)
    pData.close()
    
    map=AStarMap((100,100),svg.getDimensions())
    
    map.drawWorldPolygons(svg.getPaths(),8)
    
    
    path=map.compute2DPath((10,10), (299,399))
    #map.debugPath(path)
        

    test1=AStarMap((10,10),(50,50))
    
    test2=ASTAR_UTIL_QMap((10,5),4)
     
    test1.addZone(test2,(0,0))
    test1.debugPrint()
    
    
    test2=ASTAR_UTIL_QMap((4,4),0.5)
       
    test1.mulZone(test2,(-2,1))
    test1.debugPrint()
        
    
